deta_pitch1 = DYTdata.Pitch_FrameAngle + DYTdata.Off_target_Y;
deta_pitch2 = 1 * (deta_pitch1 - 4.59);
deta_pitch3 = constrain(deta_pitch2, 4.5 * 1, -4.5 * 1);
pitch_cmd4 = FKdata.PitchAngle + deta_pitch3;
% pitch_cmd = pitch_cmd +  4.59;
pitch_cmd5 = constrain(pitch_cmd4, 10, -10);
% figure()
plot(time, pitch_cmd5, '.');grid on;hold on;
legend("飞行模式","HK俯仰指令","matlab计算俯仰指令")

figure;
plot(time, HK2FKdata.PitchCommand)


deta_yaw = DYTdata.Yaw_FrameAngle + DYTdata.Off_target_X;
deta_yaw = 1 * (deta_yaw + 2.54);
deta_yaw = constrain(deta_yaw, 5, -5);
yaw_cmd = FKdata.YawAngle + deta_yaw;
% yaw_cmd = yaw_cmd +4.59;
yaw_cmd = mod((yaw_cmd+360), 360);
plot(time, HK2FKdata.flymode);hold on;
plot(time, HK2FKdata.YawCommand-1);hold on;
plot(time, yaw_cmd,'.');grid on;hold on;
legend("飞行模式","HK偏航指令","matlab计算偏航指令")

plot(time-0.3, FKdata.YawAngle, 'LineWidth',2);hold on;
plot(time, FKdata.Yaw_sp, 'LineWidth',2);hold on;
grid on;
legend("偏航角","偏航角指令")